Dynamic parameter identification and adaptive control with trajectory scaling for robot-environment interaction

To improve the force/position control performance of robots in contact with the environment, this paper proposes a control scheme comprising dynamic parameter identification, trajectory scaling, and computed-torque control based on adaptive parameter estimation. Based on the Newton–Euler method, the dynamic equation and its regression matrix is obtained, which is helpful to reduce the order of the model. Subsequently, the least-square method is implemented to calculate the values of the basic parameters of the dynamics. The identified dynamic parameters are used as initial values in the adaptive parameter estimation to obtain the torque, and trajectory scaling is applied to control the contact force between the robot and the environment. Finally, the dynamic parameter identification method and control algorithm are verified by conducting a simulation. The results show that the comprehensive application can help improve the control performance of robots.


Introduction
Robots are being increasingly deployed for modern industrial production. Engineers and designers have developed a wide variety of robots for various application scenarios and task requirements [1]. Given that a robot is a typical nonlinear and strongly coupled electromechanical system, the control performance has been the focus in robotics research. To improve the control accuracy of robots, the impedance control and hybrid position/force control [2,3], have been developed and widely used. An accurate dynamic model is the basis for robot control, while an inaccurate model can affect the control performance and even lead to instability [4]. Although the inertial parameters of a robot take certain constant values, some of these parameters are unknown even to the robot manufacturer because of the complexity of the robot components and assembly errors. Because of the nonlinear and strong coupling characteristics of robots, researchers have used intelligent control methods to approximate the dynamic model, such as the neural network system [5][6][7] and reinforcement learning method [8]. However, these methods can only prove the consistency and boundedness of the tracking error and cannot ensure that the estimated value converges to the real value [9][10][11]. The computational efficiency of robot dynamics Based on the collected values of the angle, angular velocity, angular acceleration, and joint torque, the dynamic parameters are calculated using the least-squares method, which is clear and simple.
2. To control the contact force, the idea of trajectory scaling is applied to the outer loop to make the end-effector of the robot follow the desired trajectory as far as possible. It is noted that the desired trajectory is varied by changing the timestamp in the trajectory generation process based on the torque error, which is helpful to indirectly realize the control of the contact force.
3. An adaptive control strategy based on the computed-torque method is adopted in the inner loop, so that the robot can adjust the dynamic parameters depending on the task and improve the control performance.
The rest of this paper is organized as follows. In Section 2, the overall scheme is introduced, including the dynamic parameter identification and control strategy design. In Section 3, we introduce the robot model and dynamic parameter identification method. In Section 4, we introduce the force control algorithms based on trajectory scaling and adaptive computation. Section 5 presents the simulation design and results. Section 6 concludes the paper.

Problem formulation
Robot grasping objects moving in tash space is a typical working scenario. Taking the widely used UR5 robot as an example, we establish the model shown in Fig 1. The structure of the parameter identification and control algorithm is shown as Fig 2, which consists of two parts. The objective of parameter identification is to obtain the inertial parameters related to the dynamics. Based on the identified dynamic parameters, the control algorithm controls the trajectory and contact force of the end-effector of the robot.
In the dynamic parameter identification module, an optimal excitation trajectory is generated based on the dynamic and kinematic models of the robot, and then the robot follows this trajectory. The joint angle and torque during this motion are collected, and the basic dynamic parameters are obtained based on the reduced-order regression matrix and least-squares algorithm. In the control scheme module, the outer loop is composed of trajectory scaling, and the inner loop is composed of an adaptive parameter estimation algorithm based on the computed torque. The objective of the former is to generate the desired trajectory with respect to the trajectory generator and the actual torque, so that the robot follows the planned trajectory as much as possible, and use this generated trajectory for the compliance of the contact force. The objective of the latter is to obtain the required torque of the robot and reduce the complexity of the algorithm using the dynamic formula based on the Newton-Euler method, in order to enable a precise control of the robot.

System model
The kinematics model of the robot is established with respect to the D-H law. After establishing the coordinate system of each link using the preposition coordinate method, we determine the transformation relationship between the i − 1 coordinate system and the i coordinate system via the translation and rotation of each coordinate system [4]. The transformation process can be used to represent the homogeneous transformation matrix iÀ 1 i R, which can be expressed as sin q i cos a iÀ 1 cos q i cos a iÀ 1 À sin a iÀ 1 À d i sin a iÀ 1 sin q i sin a iÀ 1 cos q i sin a iÀ 1 cos a iÀ 1 d i cos a iÀ 1 where a i is the length of the link, α i is the twist angle of the link, d i is the distance of the link, and q i is the angle between the two links. Lagrangian and Newton-Euler dynamics methods are typically used for robot dynamic models. For the former, the dynamic equation of the joint space is where D(q) is the inertia matrix, Cðq; _ qÞ is the centrifugal force and Coriolis force vector, and G(q) is the gravity vector. An important property of this model is its linearity, which can be expressed as where Yðq; _ q; € qÞ represents the regression matrix of the dynamics, and P represents the inertial parameter matrix. This property is the basis of the robot dynamic parameter identification and control. The Newton-Euler equation is used to derive the inverse dynamics calculation formula, which includes forward and backward processing. Forward:õ a Ci ¼ã i þε i �r Ci þõ i � ðõ i �r Ci Þ ð8Þ Backward:f where z is the unit vector in the Z axis;õ i is the angular velocity;ṽ i denotes the linear velocity; ε i denotes the angular acceleration;ã i is the linear acceleration;ã Ci is the linear acceleration of the center of mass;Ĩ Ci is the moment of inertia;F i is the inertial force;Ñ i is the moment;f i is the joint force;ñ i is the joint torque; t i is the driving moment;r Ci is the centroid coordinate. All the above variables are defined in frame i.p * i is the translation operator,

Regression function.
The robot dynamics equation established using the Newton-Euler method can be expressed using 4-11. However, it should be noted that the above formula describes the mass distribution of the link i using the mass m i and the inertia tensorĨ Ci towards its center of mass, instead of using the product of the mass m i , mass and the diameter of center of mass m ir Ci and the inertia tensor matrixĨ i to describe the mass distribution of the link. Therefore, to simplify the robot dynamics calculation with the minimum inertial parameter, it is necessary to modify the relevant dynamics calculation formula so that it contains m i , m ir Ci , andĨ i . Since the dynamic characteristics of a robot are linear functions of its inertial parameters, the identification problem of the robot parameters can be transformed to a problem of solving linear equations. The dynamic Eqs 2 and 3 can be simplified asÑ * where q, _ q, € q, and τ are known. Since Yðq; _ q; € qÞ is a known function independent of the inertial parameter P, the problem of identifying P is transformed to a problem of solving a system of linear Eq 14. Since 14 is a system of order 10n equations containing n unknowns, it is necessary to know at least q, _ q, € q, and τ among the 10 different unknowns to identify C using 14.
We define: , then we can define: Based on the parallel axis theorem:Ĩ i ¼Ĩ Ci þ m i ðr T Cir Ci I Àr Cir T Ci Þ, 9 and 15 are combined as follows: where, 17 and 18 indicate thatf i andñ i can be expressed as In the 19,f ij ¼ i j RF j reflects the influence of the dynamic properties of the link j onf i , and Þ reflects the influence of the dynamic properties onñ i . We define: From 19 and 20, we have: More generally, from 22, we have It can be concluded that where Subsequently, we have: where Therefore, we have: By comparing 14 and 26, we can write the following matrix:

Minimum parameter set.
The dynamic parameters of each link can be expressed in the form of the following vector: Only the coupling relationship between the dynamic parameters is considered, so there are only 10 parameters for each link. Two methods are mainly used to reduce the number of parameters to be identified, i.e., to obtain the minimum parameter set: QR and SVD. If a column in the matrix W is always zero, the parameters corresponding to that column have no contribution to the model. To determine the columns that are always zero, we can substitute the several sets of data to calculate the matrix W, and we can easily determine the columns whose elements are always zero.
If the robot has n joints, a total of 10n parameters are included in X. We suppose that c parameters remain after removing some of the parameters that have no contribution to the model and take r groups of data to calculate the matrix W. Moreover, we assume b parameters in the minimum parameter set of the model, namely rank(W) = b. When W is less than the rank (b < c), the decomposition QR of W is not unique. However, if after the matrix W multiplied by a column permutation matrix H, QR decomposition of WH is the only: where Q is still a positive definite matrix, T2 is a b × (c − b) matrix, and T1 is an upper triangular matrix.
In the above, the column replacement matrix H is obtained using QR decomposition, and the matrix W after column replacement is divided into two parts, as follows: where W 1 represents b independent columns, and W 2 represents c − b columns that need to be eliminated (linearly dependent on the other columns). After the column transformation of W, the corresponding parameter order in X becomes: Let XB 2 = 0, then By applying the least-squares method to the above formula, we can obtain: From 31, the following equation can be obtained:

Trajectory scaling and adaptive control algorithm design
The trajectory scaling method outputs a new desired trajectory, so it needs to be used in conjunction with the position controller. The force position control of the robot can be realized by combining the trajectory scaling method with the computed-torque control.

Trajectory scaling
In this section, the trajectory scaling method [31] is combined with the computed-torque controller to realize the force/position control of the robot. The idea of the trajectory scaling is to control the timestamp during the trajectory generation process by observing the difference between the actual joint torque of the robot and the expected joint torque, so as to change the expected trajectory and indirectly realize the force control. The implementation scheme is as follows: 1) First, based on the desired joint position, velocity, acceleration, and expected contact force, the desired joint torque is calculated using the inverse dynamic model; 2) Subsequently, through the transpose of the Jacobian matrix, the contact force is converted to the current torque of the joint, i.e., the measured torque of the joint; 3) The torque is measured minus the expected shutdown torque to obtain the torque error, whereby the timestamp in the trajectory generation process is varied to change the expected trajectory, thus indirectly realizing the control of the expected contact force. The trajectory scaling method outputs the new desired trajectory, so it needs to be used in conjunction with the position controller to achieve trajectory tracking and force control. The expected trajectory is typically a function of time. The trajectory in the joint space can be expressed as q d (t) 2 n . For each time step t i , the calculation method of the original timestamp is: where t i−1 represents the timestamp of the previous time step; Δt is the time stepping value. By introducing the scaling factor f s ðCðb rÞÞ 2 ½À 1; 1�, the time stepping value is associated with the expected trajectory and the actual contact force, where b r i represents the observed value of the external torque, which can be calculated through the expected trajectory and the measured joint torque: where τ mea represents the measured value of the joint torque obtained from the conversion of the contact force at the end of the robot through the transpose of the Jacobian matrix; b t represents the desired joint torque value corresponding to the expected trajectory of the previous time step, and the dynamic model of the mechanical arm can be used to calculate the following: Therefore, the scaling factor f s is a function of the expected trajectory ðq d ; _ q d ; € q d Þ and the measuring joint torque τ mea , so the new time step can be written as: where Cðb r i Þ is defined as where Dq i d ¼ q iþ1 d À q i d represents the difference vector of two continuous expected joint positions; τ max is a constant vector and represent the maximum torque of the joint; a b r is the sensitivity of the trajectory scaling.
Therefore, the complete form of the scaling factor f s ðCðb rÞÞ is: where k is a positive factor used to control the rate of decrease in the velocity, and is an optional dead zone. Here, the function F() 2 [0, 1] is a monotone decreasing function, which is defined as: Through the design of the above functions, the scaling factor f s will control the expected position and velocity of the robot. When the measured torque is higher than the expected moment, the scaling factor f s will decrease, making the robot to move slowly or in reverse motion until the desired moment is reached. The impact of the scaling factor f s on the motion of the robot can be characterized as follows: 1. When f s > 0, the robot moves forward (f s = 1 is consistent with the expected trajectory); 2. When f s = 0, the robot stops moving; 3. When f s < 0, the robot moves in reverse.

Adaptive control algorithm
Considering the nonlinear and strong coupling characteristics of robot dynamics, the adaptive control method based on the computed torque has been widely studied, with significant achievements made thus far. Because of the huge amount of computation, it is difficult to apply the dynamics formula based on the Lagrangian method to the control of multi-degreeof-freedom robots. The recursive formula based on the Newton-Euler method is typically used to derive the dynamics equation, then obtain the regression matrix, and construct a control law through the adaptive estimation of the inertial parameters. The algorithm can effectively reduce the complexity of the control law and has little relationship with the degree of freedom of the robot.
The dynamic equations of the robot are described in 2 and 3. The variables y and q r are respectively introduced and defined as follows: where γ � 0, and we can deduce the following formula: According to 2 and 3, one can obtain Subsequently: The following controller and adaptive law are designed to ensure the global stability of the system: The stability proof of the control law is given below. We defineP ¼ b P À P, and then the Lyapunov function can be written as: By differentiating 47 with respect to time, we can obtain: Based on the linearity and anti-symmetry of the robot dynamics equation, we arrive at: Hence, it is proven that the robot is stable when using the control law.

Finding the regression matrix
From 17-28, we find that the regression matrix Y can be expressed as a function ofõ i ,ε i , and v i . Therefore, the expressions ofõ ir ,ε ir , andṽ ir for obtaining Yðq; _ q; _ q r ; € q r Þ are given as follows:õ Thus, we have Yðq; _ q; _ q r ; € q r Þ. Remark 1 Using regression matrix can implement the known information in dynamics as much as possible, and take the identified parameters as the initial value of the adaptive controller. In this case, the calculated dynamic model is more accurate. Relatively speaking, the adaptive control method does not require the initial value of dynamic parameters, so the dynamic model error is large when the robot is just running. It is worth mentioning that, another common scheme of robot adaptive control is to use fuzzy logic system and radial basis function neural network combined with adaptive estimator to deal with the uncertainty of dynamic model. In this case, most known information of dynamics can not be used; Because the initial value of the estimated parameter needs to be set according to experience, and it will bring large model error before approaching the true value. However, the control scheme proposed in this paper does not have this problem.

Numerical simulation
To verify the feasibility of the proposed control method, MATLAB/Simulink is applied to build the robot platform with UR5 for real-time simulation. UR5 has six links and six joints, whose parameters are shown in Table 1. The simulation experiment in this chapter firstly verifies the validity of parameter identification algorithm and obtains the identification results of dynamic parameters. Then, these identification results are used as the initial state of the adaptive parameter estimates to verify the effectiveness of the control algorithm combining trajectory stretching and adaptive.

Verification of parameter identification method
The aim of the excitation trajectory design is to excite the robot sufficiently so as to obtain the dynamic parameters of the robot more accurately. For a given robot, the optimal excitation trajectory is theoretically the one that minimizes the error of dynamic parameter identification using the excitation trajectory. The type of excitation trajectory is the finite-term Fourier-series trajectory, and the parameters of the excitation trajectory need to be optimized depending on the actual situation. Since the Fourier-series trajectory is used as the excitation trajectory, the Fourier-series trajectory of joint i is expressed as follows: where q i (t), _ q i ðtÞ, and € q i ðtÞ are respectively the angle, angular velocity, and angular acceleration of joint i at time t. ω f is the interval angular frequency, k is the number of harmonic terms of the Fourier series, a i,k and b i,k are the amplitudes of the sine and cosine terms of joint i. In this simulation, ω f = 0.1π, and the coefficient of Fourier series is obtained by dynamic programming. Finally, the coefficient combinations, listed in Table 2, can be obtained; Fig 3 shows the excitation trajectories of each joint. The model of the robot, listed in Table 1, obtained using the Newton-Euler method contains 60 dynamic parameters. By using the dimensionality reduction method of the model proposed in this paper and grouping the dynamic parameters, we can obtain a set containing 51 dynamic parameters, including 36 fully identifiable parameters and 15 identified parameters after linear combination. The remaining nine are unidentifiable parameters.
In this study, the dynamic parameters obtained via the parameter identification method are used for the adaptive control of the selected robot. We input the numerical value of the excitation trajectory to the robot, and then by measuring the joint angle, angular velocity, angular acceleration, and joint torque of the robot, we can complete the dynamic parameter identification. The identification method proposed in this paper is adopted; Table 3 lists the dynamic parameters obtained.   in the measured and computed torque. As shown in Figs 5 and 6, the measured and computed torques are largely the same, with an error of less than 10 −12 Nm. The simulation results show that the inertial parameters identified are effective for the excitation trajectory and joint torque. To further verify the validity of the identified inertial parameters, another set of excitation trajectories is used for verification.

Verification of control algorithm
Based on the trajectory scaling and adaptive control algorithm, we carry out a simulation verification in this part. The same robot model is used for the dynamic parameter identification, as listed in Table 1.
In the trajectory scaling module, the sensitivity of the scaling α = 10, the speed decreasing factor k = 1, and optional dead zone Γ = 0.5. In the adaptive torque control module,γ = 20eyes (6),K p = diag( [10,20,10, To further verify the effect of dynamic parameter identification, the identified results are used as initial values for the adaptive parameter estimation as part of a re-simulation. Figs 12-14 show the results, which respectively represent the trajectory tracking results and joint torque. The control effect is evident because the initial value is non-zero. The above simulation results show that the trajectory tracking error of the robot is very small and that the control precision is very high when using data identified from the dynamic parameters. This also proves the effectiveness of the dynamic parameter identification method and the robot control method.

Conclusion
In this study, a robot dynamics model is established using the Newton-Euler method to determine the regression matrix and parameter vectors. The order of the regression matrix is reduced, and the minimum parameter set is obtained. The validity of the method is proven using the verification trajectory. We employe force control for the outer loop based on trajectory scaling and position control for the inner loop based on the adaptive torque method to achieve a high-precision control performance and compliance control of the robot. The proposed algorithm is verified by conducting a simulation. As part of future work, we plan to verify our approach on an experimental platform suitable for a real robot and further develop a compliant control algorithm for the force.